import math

import numpy as np


def find_position_of_ones(decimal_num):
    # 将十进制数转换为二进制数，并反转二进制字符串
    binary_str = bin(decimal_num)[2:][::-1]

    # 创建一个空列表来存储所有1的位置
    positions = []

    # 遍历二进制字符串，查找所有1的位置
    for i in range(len(binary_str)):
        # 如果当前位为1，则将它的位置添加到位置列表中
        if int(binary_str[i]) & 1 == 1:
            positions.append(i)

    # 返回位置列表
    return positions

def change_bit(decimal, position):
    binary = bin(decimal)[2:]  # 转换为二进制字符串，去掉前缀 '0b'
    binary = binary.rjust(7, '0')
    print(binary)
    binary_list = list(binary)  # 将字符串转换为列表
    print(binary_list)
    binary_list[-position-1] = '1'
    print(binary_list)
    binary = ''.join(binary_list)  # 将列表转换回字符串
    print(binary)
    return int(binary, 2)  # 将二进制字符串转换为十进制数并返回

class Robot:
    def __init__(self, pos, target_pos, radius, max_speed, max_angular_speed):
        self.pos = pos
        self.target_pos = target_pos
        self.radius = radius
        self.max_speed = max_speed
        self.max_angular_speed = max_angular_speed

class LineSegment:
    def __init__(self, start, end):
        self.start = start
        self.end = end

    def intersects(self, other):
        # 计算两条线段的四个端点
        p1, q1 = self.start, self.end
        p2, q2 = other.start, other.end

        # 计算两条线段的方向向量和垂直向量
        v1, v2 = q1 - p1, q2 - p2
        u1, u2 = np.array([-v1[1], v1[0]]), np.array([-v2[1], v2[0]])

        # 计算两条线段在垂直方向上的投影长度
        d1 = np.dot(u1, p2 - p1)
        d2 = np.dot(u1, q2 - p1)
        d3 = np.dot(u2, p1 - p2)
        d4 = np.dot(u2, q1 - p2)

        # 如果两条线段的投影有重叠，则说明它们相交
        if d1 * d2 < 0 and d3 * d4 < 0:
            return True
        elif d1 == 0 and self._on_segment(p2):
            return True
        elif d2 == 0 and self._on_segment(q2):
            return True
        elif d3 == 0 and other._on_segment(p1):
            return True
        elif d4 == 0 and other._on_segment(q1):
            return True
        else:
            return False

    def _on_segment(self, point):
        # 检查一个点是否在当前线段上
        return (min(self.start[0], self.end[0]) <= point[0] <= max(self.start[0], self.end[0])
                and min(self.start[1], self.end[1]) <= point[1] <= max(self.start[1], self.end[1]))

if __name__ == '__main__':
    robot1 = Robot(np.array([0, 0]), np.array([10, 10]), 0.5, 6, math.pi)
    robot2 = Robot(np.array([1, 1]), np.array([10, 5]), 0.5, 6, math.pi)
    robot3 = Robot(np.array([2, 2]), np.array([5, 5]), 0.5, 6, math.pi)
    robot4 = Robot(np.array([3, 3]), np.array([5, 10]), 0.5, 6, math.pi)
    robots = [robot1, robot2, robot3, robot4]
    # value = find_position_of_ones(48)
    # print(value)
    # print(6 not in value)
    # list = [-1, -1, -1, -1]
    # list[2] = 5
    # list = [1, 5, 6, 5]
    # print(list.index(5))
    # 十进制数
    # decimal_num = 48
    # new = change_bit(decimal_num, 5)
    # print(new)
    # current_pos = [np.array([0, 0]), np.array([0, 2])]
    # next_pos = [np.array([2, 2]), np.array([5, 7])]
    # line1 = LineSegment(current_pos[0], next_pos[0])
    # line2 = LineSegment(current_pos[1], next_pos[1])
    # print(line1.intersects(line2))
